#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>

// 定义2D点结构
typedef struct {
    float x;
    float y;
} Point2f;

// 定义仿射变换矩阵 [a, b, tx; c, d, ty]
typedef struct {
    float a, b, tx;
    float c, d, ty;
} AffineTransform;

// 计算由三对点确定的仿射变换
int computeAffineTransform(const Point2f src[], const Point2f dst[], AffineTransform *transform) {
    // 构建线性方程组 Ax = b
    float A[6][6] = {
        {src[0].x, src[0].y, 1, 0, 0, 0},
        {0, 0, 0, src[0].x, src[0].y, 1},
        {src[1].x, src[1].y, 1, 0, 0, 0},
        {0, 0, 0, src[1].x, src[1].y, 1},
        {src[2].x, src[2].y, 1, 0, 0, 0},
        {0, 0, 0, src[2].x, src[2].y, 1}
    };

    float b[6] = {
        dst[0].x, dst[0].y,
        dst[1].x, dst[1].y,
        dst[2].x, dst[2].y
    };

    // 使用高斯消元法求解方程组
    for (int i = 0; i < 6; i++) {
        // 寻找主元
        int pivot = i;
        for (int j = i + 1; j < 6; j++) {
            if (fabs(A[j][i]) > fabs(A[pivot][i])) {
                pivot = j;
            }
        }

        // 交换行
        if (pivot != i) {
            for (int j = 0; j < 6; j++) {
                float temp = A[i][j];
                A[i][j] = A[pivot][j];
                A[pivot][j] = temp;
            }
            float temp = b[i];
            b[i] = b[pivot];
            b[pivot] = temp;
        }

        // 检查矩阵是否接近奇异
        if (fabs(A[i][i]) < 1e-6) {
            return 0; // 失败：点可能共线
        }

        // 归一化当前行
        float divisor = A[i][i];
        for (int j = i; j < 6; j++) {
            A[i][j] /= divisor;
        }
        b[i] /= divisor;

        // 消元
        for (int j = i + 1; j < 6; j++) {
            float factor = A[j][i];
            for (int k = i; k < 6; k++) {
                A[j][k] -= factor * A[i][k];
            }
            b[j] -= factor * b[i];
        }
    }

    // 回代求解
    for (int i = 5; i >= 0; i--) {
        for (int j = i + 1; j < 6; j++) {
            b[i] -= A[i][j] * b[j];
        }
    }

    // 提取解作为仿射变换参数
    transform->a = b[0];
    transform->b = b[1];
    transform->tx = b[2];
    transform->c = b[3];
    transform->d = b[4];
    transform->ty = b[5];

    return 1; // 成功
}

// 计算点经过仿射变换后的位置
Point2f transformPoint(const Point2f point, const AffineTransform transform) {
    Point2f result;
    result.x = transform.a * point.x + transform.b * point.y + transform.tx;
    result.y = transform.c * point.x + transform.d * point.y + transform.ty;
    return result;
}

// 计算点到变换的重投影误差
float computeReprojectionError(const Point2f src, const Point2f dst, const AffineTransform transform) {
    Point2f transformed = transformPoint(src, transform);
    float dx = transformed.x - dst.x;
    float dy = transformed.y - dst.y;
    return sqrtf(dx * dx + dy * dy);
}

// 随机数生成器（0到n-1之间的整数）
int randomInt(int n) {
    return rand() % n;
}

// 使用RANSAC算法估计仿射变换
int estimateAffine2DRansac(
    const Point2f *srcPoints,
    const Point2f *dstPoints,
    int numPoints,
    AffineTransform *bestTransform,
    int *inliers,
    int *numInliers,
    float reprojectionThreshold,
    int maxIters,
    float confidence
) {
    if (numPoints < 3) return 0;

    // 初始化随机数生成器
    srand(time(NULL));

    // 最佳模型参数
    int bestInlierCount = 0;
    int *inlierMask = (int *)malloc(numPoints * sizeof(int));
    if (!inlierMask) return 0;

    // RANSAC迭代
    for (int iter = 0; iter < maxIters; iter++) {
        // 1. 随机选择3个点
        Point2f srcSample[3], dstSample[3];
        int indices[3];

        for (int i = 0; i < 3; i++) {
            int idx;
            // 确保选择的点不重复
            do {
                idx = randomInt(numPoints);
                for (int j = 0; j < i; j++) {
                    if (idx == indices[j]) {
                        idx = -1;
                        break;
                    }
                }
            } while (idx == -1);

            indices[i] = idx;
            srcSample[i] = srcPoints[idx];
            dstSample[i] = dstPoints[idx];
        }

        // 2. 计算仿射变换
        AffineTransform transform;
        if (!computeAffineTransform(srcSample, dstSample, &transform)) {
            continue; // 点共线，跳过
        }

        // 3. 计算内点数量
        int inlierCount = 0;
        for (int i = 0; i < numPoints; i++) {
            float error = computeReprojectionError(srcPoints[i], dstPoints[i], transform);
            if (error < reprojectionThreshold) {
                inlierMask[i] = 1;
                inlierCount++;
            } else {
                inlierMask[i] = 0;
            }
        }

        // 4. 更新最佳模型
        if (inlierCount > bestInlierCount) {
            bestInlierCount = inlierCount;
            *bestTransform = transform;

            // 提前终止条件
            if (bestInlierCount >= numPoints) break;

            // 更新迭代次数
            float pNoOutliers = pow((float)(numPoints - bestInlierCount) / numPoints, 3);
            float eps = 1e-6;
            if (1.0 - pNoOutliers < eps) pNoOutliers = 1.0 - eps;
            float neededIters = log(1.0 - confidence) / log(pNoOutliers);
            if (neededIters < maxIters) {
                maxIters = (int)neededIters + 1;
            }
        }
    }

    // 复制内点掩码
    if (inliers && bestInlierCount > 0) {
        memcpy(inliers, inlierMask, numPoints * sizeof(int));
    }

    if (numInliers) {
        *numInliers = bestInlierCount;
    }

    free(inlierMask);
    return bestInlierCount >= 3;
}

// 使用最小二乘法优化仿射变换
int refineAffineTransform(
    const Point2f *srcPoints,
    const Point2f *dstPoints,
    int numPoints,
    const int *inliers,
    AffineTransform *transform
) {
    // 收集所有内点
    int inlierCount = 0;
    for (int i = 0; i < numPoints; i++) {
        if (inliers[i]) inlierCount++;
    }

    if (inlierCount < 3) return 0;

    // 分配内存
    Point2f *srcInliers = (Point2f *)malloc(inlierCount * sizeof(Point2f));
    Point2f *dstInliers = (Point2f *)malloc(inlierCount * sizeof(Point2f));
    if (!srcInliers || !dstInliers) {
        free(srcInliers);
        free(dstInliers);
        return 0;
    }

    // 复制内点
    int idx = 0;
    for (int i = 0; i < numPoints; i++) {
        if (inliers[i]) {
            srcInliers[idx] = srcPoints[i];
            dstInliers[idx] = dstPoints[i];
            idx++;
        }
    }

    // 构建超定方程组并求解（最小二乘解）
    // 这里简化实现，实际应使用SVD或正规方程求解
    int success = computeAffineTransform(srcInliers, dstInliers, transform);

    free(srcInliers);
    free(dstInliers);
    return success;
}

// 示例使用
int main() {
    // 创建示例点集
    Point2f srcPoints[4] = {
        {0, 0}, {100, 0}, {100, 100}, {0, 100}
    };

    // 目标点集（经过旋转+平移）
    Point2f dstPoints[4] = {
        {50, 50}, {120, 30}, {100, 100}, {30, 80}
    };

    // 估计仿射变换
    AffineTransform transform;
    int *inliers = (int *)calloc(4, sizeof(int));
    int numInliers;

    if (estimateAffine2DRansac(
        srcPoints, dstPoints, 4, &transform, inliers, &numInliers,
        3.0f, 1000, 0.99f
    )) {
        // 优化变换
        refineAffineTransform(srcPoints, dstPoints, 4, inliers, &transform);

        printf("估计的仿射变换矩阵:\n");
        printf("[%f, %f, %f]\n", transform.a, transform.b, transform.tx);
        printf("[%f, %f, %f]\n", transform.c, transform.d, transform.ty);
        printf("内点数量: %d\n", numInliers);
    } else {
        printf("无法估计仿射变换\n");
    }

    free(inliers);
    return 0;
}
